/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.thesis.utils;

import java.util.*;

/**
 *
 * @author Aya Hikaru
 */
public class VelocityEstimate {
    private boolean DEBUGGING = true;

    private void adjustLastBySquaredFit() {
        
        if (velocityList.size() < 1)
            return;
        VelocityElement veL = velocityList.getLast();
        if (velocityList.size() <= 2)
        {
            if (veL.velocity > MAXALLOWVELOCITY) {
                veL.velocity = MAXALLOWVELOCITY;
                if (DEBUGGING) {
                    System.out.println("++++++++++++++++++" + veL.velocity * 3600 + "-----" + MAXALLOWVELOCITY* 3600 );
                }
            }
            return;
        }
        int from = veL.timeStamp - ADJUSTLASTWINDOWLENGTH;
        double squaredFit = returnVelocityLeastSquaredFitLine(veL.timeStamp, (from < 0)?0:from, veL.timeStamp);
        if (Math.abs(squaredFit - veL.velocity) > MAXALLOWDIFF && squaredFit < veL.velocity && squaredFit > 1 && squaredFit < MAXALLOWVELOCITY)
        {       
            if (DEBUGGING) {
            System.out.println("++++++++++++++++++" + veL.velocity* 3600  + "-----" + squaredFit* 3600 );
            }
            veL.velocity = squaredFit;
        } else {
            if (veL.velocity > MAXALLOWVELOCITY) {
                veL.velocity = MAXALLOWVELOCITY;
                if (DEBUGGING) {
                    System.out.println("++++++++++++++++++" + veL.velocity * 3600 + "-----" + MAXALLOWVELOCITY* 3600 );
                }
            }
        }
    }

    private double MAXALLOWVELOCITY = (35 / 3600.0);
    private double MAXALLOWDIFF = (10 / 3600.0);
    private int ADJUSTLASTWINDOWLENGTH = 600;
    private BusControl buscontroller;
    protected LinkedList<VelocityElement> velocityList = new LinkedList<VelocityElement>();
    private Hashtable<Character, LinkedList<Character>> computedpair = new Hashtable<Character, LinkedList<Character>>();
    private Hashtable<Character, LinkedList<Character>> lastaddedcomputedpair = new Hashtable<Character, LinkedList<Character>>();
    LinkedList<Character> lastBusstops = new LinkedList<Character>();
    int lastTimeStamp = -1;
    int beforelastTimeStamp = -1;
    LinkedList<Character> beforelastBusstops = new LinkedList<Character>();

    public VelocityEstimate(BusControl bc) {
        buscontroller = bc;
    }
    public VelocityEstimate() {
    	//TODO: check
    }
    private String printLinkedListCharacter(LinkedList< Character> llc) {
        String result = "{";
        for (char c : llc) {
            result += ((int) (c)) + ",";
        }
        result = result.substring(0, result.length() - 1);
        result += "}";
        return result;

    }

    public void Add(int timeStamp, LinkedList<Character> BusStops, short[] routeList) {
        Enumeration<Character> f;
        if (DEBUGGING) {
            System.out.println("ADD AT " + timeStamp + " LIST: " + this.printLinkedListCharacter(BusStops));
            System.out.println("BEFORE: ");
            System.out.println("computedpair: ");
            f = computedpair.keys();
            while (f.hasMoreElements()) {
                Character me = f.nextElement();
                System.out.println("==" + (int) me + "==" + this.printLinkedListCharacter(computedpair.get(me)));
            }
        }
        double tmp = 0;
        if (lastBusstops.size() == 0 || lastTimeStamp == -1 || BusStops.size() == 0 || routeList.length == 0) {
            velocityList.add(new VelocityElement(timeStamp, 0.0));
        } else {
            // check if the same
            boolean theSame = true;
            for (char currBSID : BusStops) {
                if (!lastBusstops.contains(currBSID)) {
                    theSame = false;
                    break;
                }
            }
            if (theSame == true) {
                if (velocityList.size() == 1 && beforelastTimeStamp == -1) {
                    lastaddedcomputedpair.clear();
                    velocityList.removeLast();
                    velocityList.add(new VelocityElement(timeStamp, 0.0));
                } else {
                    Enumeration<Character> e = lastaddedcomputedpair.keys();
                    while (e.hasMoreElements()) {
                        Character me = e.nextElement();
                        LinkedList<Character> tmpllc;
                        tmpllc = computedpair.get(me);
                        for (Character c : lastaddedcomputedpair.get(me)) {
                            tmpllc.remove(c);
                        }
                        if (tmpllc.size() == 0) {
                            computedpair.remove(me);
                        }
                    }
                    if (DEBUGGING) {
                        System.out.println("THESAMECASE: ");
                        System.out.println("computedpair: ");
                        f = computedpair.keys();
                        while (f.hasMoreElements()) {
                            Character me = f.nextElement();
                            System.out.println("==" + (int) me + "==" + this.printLinkedListCharacter(computedpair.get(me)));
                        }
                        System.out.println("lastcomputedpair: ");
                        f = lastaddedcomputedpair.keys();
                        while (f.hasMoreElements()) {
                            Character me = f.nextElement();
                            System.out.println("==" + (int) me + "==" + this.printLinkedListCharacter(lastaddedcomputedpair.get(me)));
                        }
                    }
                    lastaddedcomputedpair.clear();
                    tmp = calculateVelocity(beforelastTimeStamp, beforelastBusstops, timeStamp, BusStops, routeList);
                    velocityList.removeLast();
                    velocityList.add(new VelocityElement(timeStamp, tmp));
                    adjustLastBySquaredFit();
                }
            } else {
                lastaddedcomputedpair.clear();
                tmp = calculateVelocity(lastTimeStamp, lastBusstops, timeStamp, BusStops, routeList);
                velocityList.add(new VelocityElement(timeStamp, tmp));
                beforelastBusstops = lastBusstops;
                beforelastTimeStamp = lastTimeStamp;
            }
        }
        if (tmp > MAXALLOWVELOCITY)
                    adjustLastBySquaredFit();
        lastTimeStamp = timeStamp;
        lastBusstops = new LinkedList<Character>();
        for (char BSID : BusStops) {
            lastBusstops.add(BSID);
        }
        if (DEBUGGING) {
            System.out.println("AFTER: ");
            System.out.println("computedpair: ");
            f = computedpair.keys();
            while (f.hasMoreElements()) {
                Character me = f.nextElement();
                System.out.println("==" + (int) me + "==" + this.printLinkedListCharacter(computedpair.get(me)));
            }
            System.out.println("lastcomputedpair: ");
            f = lastaddedcomputedpair.keys();
            while (f.hasMoreElements()) {
                Character me = f.nextElement();
                System.out.println("==" + (int) me + "==" + this.printLinkedListCharacter(lastaddedcomputedpair.get(me)));
            }


            System.out.println("=======VELOCITY LIST=========");
            for (VelocityElement ve : velocityList) {
                System.out.print(ve.timeStamp + "==" + ve.velocity * 3600 + " ");
            }
            System.out.println();
            System.out.println("+++++VELOCITY LIST END+++++++");
        }
    }

    private double calculateVelocity(int fromTimeStamp, LinkedList<Character> fromBusStops, int toTimeStamp, LinkedList<Character> toBusStops, short[] routeList) {

        double Sum = 0;
        double distance = 0;
        int count = 0;
        BusRoute br = null;
        BusStop bs = null;
        int previndex = 0;
        int currentindex = 0;
        LinkedList<Character> tmpllcinlast = null;
        LinkedList<Character> tmpllc = null;
        for (int i = 0; i < routeList.length; i++) {

            br = buscontroller.getBusRoute(routeList[i]);
            if (br != null) {
                for (char prevBSID : fromBusStops) {
                    previndex = br.BusStopIDList.indexOf(prevBSID);

                    if (previndex != -1) {
                        for (char currBSID : toBusStops) {
                            currentindex = br.BusStopIDList.indexOf(currBSID);
                            if (DEBUGGING) {
                                System.out.println("BusRoute: " + routeList[i] + " from BusStop: " + (int) prevBSID + " at " + previndex + " to BusStop: " + (int) currBSID + " at " + currentindex);
                            }
                            if (currentindex > previndex) {
                                tmpllc = computedpair.get(prevBSID);
                                tmpllcinlast = lastaddedcomputedpair.get(prevBSID);
                                if (tmpllc == null) {
                                    tmpllc = new LinkedList<Character>();
                                }
                                if (tmpllcinlast == null) {
                                    tmpllcinlast = new LinkedList<Character>();
                                }
                                if (!tmpllc.contains(currBSID)) {
                                    tmpllc.add(currBSID);
                                    if (tmpllcinlast.contains(currBSID)) {
                                        System.err.println("SUM TING RONG");
                                    }
                                    tmpllcinlast.add(currBSID);
                                    computedpair.put(prevBSID, tmpllc);
                                    lastaddedcomputedpair.put(prevBSID, tmpllcinlast);

                                    distance = buscontroller.getDistanceBetweenBusStops(prevBSID, currBSID);
                                    if (DEBUGGING) {
                                        System.out.println("distance: " + distance);
                                    }
                                    if (distance < .000001) {
                                        System.err.println("BusRoute: " + routeList[i] + " from BusStop: " + (int) prevBSID + " at " + previndex + " to BusStop: " + (int) currBSID + " at " + currentindex + "Distance is 0?");

                                    } else {
                                        Sum += distance;
                                        count++;
                                        if (DEBUGGING) {
                                            System.out.println("<<added COUNT: " + count + " SUM: " + Sum);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }

        if (count > 1) {
            Sum /= count;
        }
        if (count == 0) {
            System.err.println("COUNT = 0???");
            return 0;
        } else {
            if (DEBUGGING) {
                System.out.println("FINAL: SECONDS FROM " + fromTimeStamp + " TO " + toTimeStamp + " SUM: " + Sum + " RESULT: " + (Sum / (toTimeStamp - fromTimeStamp)) * 3600);
            }
//            if ((Sum / (toTimeStamp - fromTimeStamp)) > MAXALLOWVELOCITY) {
//                if (DEBUGGING) {
//                    System.out.println("RETURN MAXALLOW:" + MAXALLOWVELOCITY);
//                }
//                
//                return MAXALLOWVELOCITY;
//                
//            } else 
//            {
//                return (Sum / (toTimeStamp - fromTimeStamp));
//            }
            return (Sum / (toTimeStamp - fromTimeStamp));
        }
    }

    public double returnVelocity(int FromTimeStamp, int ToTimeStamp) {
        double Sum = 0;
        int count = 0;
        if (FromTimeStamp < 0) {
            FromTimeStamp = 0;
        }
        int prevTimeStamp = -1;
        for (VelocityElement ve : velocityList) {
        	if (velocityList.size() > 3 &&ve.timeStamp == velocityList.getLast().timeStamp && (ve.timeStamp - velocityList.get(velocityList.size()-2).timeStamp) < 120)
        		break;
            if (ve.timeStamp <= FromTimeStamp) {
                
            } else if (ve.timeStamp <= ToTimeStamp) {
                if (prevTimeStamp <= FromTimeStamp) {
                    if (ve.velocity > 0.000001)
                    {
                    Sum += (ve.velocity * (ve.timeStamp - FromTimeStamp));
                    count += (ve.timeStamp - FromTimeStamp);
                    }
                } else {
                    if (ve.velocity > 0.000001)
                    {
                    Sum += (ve.velocity * (ve.timeStamp - prevTimeStamp));
                    count += (ve.timeStamp - prevTimeStamp);
                    }
                }
            } else {
                if (prevTimeStamp >= ToTimeStamp) {
                    break;
                } else {
                    if (ve.velocity > 0.000001)
                    {
                    Sum += (ve.velocity * (ToTimeStamp - prevTimeStamp));
                    count += (ToTimeStamp - prevTimeStamp);
                    }
                    break;
                    
                }
            }
            prevTimeStamp = ve.timeStamp;
        }
        if (count == 0) {
            return 0;
        } else {
            return (Sum / count);
        }
    }

    public double returnVelocityNaive(int TimeStamp) {
        for (VelocityElement ve : velocityList) {
            if (ve.timeStamp == TimeStamp) {
                return ve.velocity;
            }
        }
        return (returnVelocity(TimeStamp - 100, TimeStamp + 100));
    }

    public double returnVelocityLeastSquaredFitLine(int TimeStamp, int fromTimeStamp, int toTimeStamp) {
        LinkedList<VelocityElement> tmp = new LinkedList<VelocityElement>();
        for (VelocityElement ve : velocityList) {
            //if (ve.timeStamp == TimeStamp)
            //    return ve.velocity;
            if (ve.timeStamp >= fromTimeStamp && ve.timeStamp <= toTimeStamp && ve.velocity > 0.000001) {
                tmp.add(ve);
            }
        }
        
        if (tmp.size() == 0) {
            return 0;
        } else {
            if (tmp.getLast().velocity >= MAXALLOWVELOCITY)
                tmp.removeLast();
            if (tmp.size() == 0) {
                return 0;
            }
            double summationX = 0;
            double summationY = 0;
            double summationXY = 0;
            double summationX2 = 0;
            for (VelocityElement ve : tmp) {
                summationX += ve.timeStamp;
                summationY += ve.velocity;
                summationXY += ve.timeStamp * ve.velocity;
                summationX2 += ve.timeStamp * ve.timeStamp;
            }
            double slope = (summationXY - (summationX * summationY / tmp.size()))
                    / (summationX2 - (summationX * summationX / tmp.size()));
            double b = summationY / tmp.size() - slope * summationX / tmp.size();
            return slope * TimeStamp + b;
        }
    }

    public void Print() {
        for (VelocityElement ve : velocityList) {
            System.out.println("At timestamp: " + ve.timeStamp + " with velocity : " + ve.velocity);
        }
    }
}
